#include "FrameMaker.h"
#include <cmath>
#include <Inventor/SbTesselator.h>
#include <Inventor/nodes/SoText3.h>
#include <Inventor/nodes/SoFont.h>
#include <Inventor/nodes/SoTransform.h>
#include "Triangulate.h"
#include "BaseConfigs.h"
#include <assert.h>

namespace DDAGui{
/////////////////////////////////////////////////
/////
/////  graph block
/////
////////////////////////////////////////////////
using namespace std;
double maxStress = 0;
extern void msg(const char* str);
extern void msg2File(const char* str);

// namespace ScaleSpace
// {
// 	double realTimeDisplacement = 0;
// 	double accumulatedDisplacement = 0;
// 	
// }


namespace FrameMakerSpace
{
	bool ifDisplayWithQuadraticCurve = true;
	int frameIdx = 0;
	double scale=1;
	std::vector<double> displacements;
	std::vector<std::vector<SbVec3f> > mpsCoords;
}

void setScale4Display(double scale)
{
	FrameMakerSpace::scale = scale;
}

double getScale4Display()
{
	return FrameMakerSpace::scale;
}

void setFrameIdx(int idx)
{	FrameMakerSpace::frameIdx=idx;	}

int getFrameIdx()
{	return FrameMakerSpace::frameIdx;	}

void setIfDisplayWithQuadraticCurve(bool flag)
{	FrameMakerSpace::ifDisplayWithQuadraticCurve = flag;	}

bool getIfDisplayWithQuadraticCurve()
{	return FrameMakerSpace::ifDisplayWithQuadraticCurve;	}

// This will update accumulated displacement at same time
// For class "DF"
void updateRealTimeDisplacement(double value)
{
	double tmp = FrameMakerSpace::displacements.back()+value;
	FrameMakerSpace::displacements.push_back(tmp);
//	FrameMakerSpace::displacements += FrameMakerSpace::realTimeDisplacement;
}

double getRealTimeDisplacement()	
{
	if(FrameMakerSpace::frameIdx>0)
		return FrameMakerSpace::displacements[FrameMakerSpace::frameIdx]
				-FrameMakerSpace::displacements[FrameMakerSpace::frameIdx-1];
	else
		return FrameMakerSpace::displacements[0]; 
}

double getAccumulatedDisplacement()	
{	return FrameMakerSpace::displacements[FrameMakerSpace::frameIdx]; }

void clearDisplacements()
{
	FrameMakerSpace::displacements.resize(1);
	FrameMakerSpace::displacements[0] = 0.1;
}


std::vector<double>& getDisplacements()
{
	return FrameMakerSpace::displacements;
}

bool loadDisplacements(int framesNum)
{
	framesNum +=1; // the first frame's index is 0, num(all frames) = framesNum+1
	clearDisplacements();

	char path[300];
	sprintf(path, "%s/displacement.dg", DDAGui::currentProjectPath);
	FILE* tmpFile = fopen(path, "rb");
	if(!tmpFile) return false;

	double tmp;
	FrameMakerSpace::displacements.resize(framesNum+1);
	int i=0;

	fscanf(tmpFile, "%lf", &tmp);
	FrameMakerSpace::displacements[0]=tmp;
	while(!feof(tmpFile) && i++<framesNum)
	{
		fscanf(tmpFile, "%lf", &tmp);
		FrameMakerSpace::displacements[i]=tmp+FrameMakerSpace::displacements[i-1];
	}
	fscanf(tmpFile, "%s", path);  // there is a '\n' at last
	if( !feof(tmpFile) || i<framesNum )
		return false;
	return true;
}
void clearMeasuredPointCoords()
{
	for(int i=0; i<FrameMakerSpace::mpsCoords.size() ; i++)
		FrameMakerSpace::mpsCoords[i].resize(0);
	FrameMakerSpace::mpsCoords.resize(0);
}

void initMeasuredPointCoords(int mpsNum, int framesNum)
{
	clearMeasuredPointCoords();
	FrameMakerSpace::mpsCoords.resize(mpsNum);
	for( int i=0 ; i<FrameMakerSpace::mpsCoords.size() ; i++)
		FrameMakerSpace::mpsCoords[i].resize(framesNum+2); // avoid overflow
}

bool loadMeasuredPointCoords(int mpsNum, int framesNum)
{
	framesNum +=1; // the first frame's index is 0, num(all frames) = framesNum+1

	char path[300];
	sprintf(path, "%s/measuredPointCoordinates.dg", DDAGui::currentProjectPath);
	FILE* tmpFile = fopen(path, "rb");
	if(!tmpFile) return false;

	double a, b;
	char str[20], str2[20];

	initMeasuredPointCoords(mpsNum, framesNum);

	int i=0;
	for( i=0; !feof(tmpFile) && i<framesNum ; i++)
	{
		fscanf(tmpFile, "%s %s", str, str2); // <<<step>>> n
// 		ASSERT(str[0]=='<');
		assert(str[0]=='<');

		for( int j=0; j<mpsNum ; j++)
		{
			fscanf(tmpFile, "%lf  %lf", &a, &b);
			FrameMakerSpace::mpsCoords[j][i].setValue(a, b, 0.1);
		}
	}
	fscanf(tmpFile, "%s", path);  // there is a '\n' at last
	if( !feof(tmpFile) || i<framesNum )
	{
		msg2File(path);
		msg2File("file not complete.\n");
		return false;
	}
	return true;
}

void addMeasuredPointCoordinate4Idx(int pIdx, double x, double y)
{
	int frameIdx = getFrameIdx();
	FrameMakerSpace::mpsCoords[pIdx][frameIdx].setValue(x, y, 0.1);
}

std::vector<SbVec3f>& getMeasuredPointCoords4Idx(int mpIdx)
{
	return FrameMakerSpace::mpsCoords[mpIdx];
}


void scanAndUpdateMaxStress(FrameData* frame)
{
    vector<Block>& blocks = frame->blocks;
    double stress = blocks[0].stressX; 
    for(vector<Block>::const_iterator it = blocks.begin() ; it!=blocks.end(); it++)
    {
        stress = stress < fabs(it->stressX) ? fabs(it->stressX) : stress;
        stress = stress < fabs(it->stressY) ? fabs(it->stressY) : stress;
        stress = stress < fabs(it->stressXY) ? fabs(it->stressXY) : stress;
    }    
    maxStress = stress > maxStress ? stress : maxStress;

//    char str[200];
//    sprintf(str ,"current max stress : %f\n", maxStress);
//    msg(str);
}

GraphBlock::GraphBlock()
{
	////////////////////////////////
	// block graph
	////////////////////////////////
	SoSeparator* polygon = new SoSeparator();
	polygon->ref();


	norm = new SoNormal();
	norm->ref();
	norm->vector.setValue(0 , 0 , 1.0);
    polygon->addChild(norm);

	normalBinding = new SoNormalBinding();
	normalBinding->ref();
	normalBinding->value = SoNormalBinding::PER_FACE;
	polygon->addChild(normalBinding);

	polygonColors = new SoMaterial() ;
	polygonColors->ref();
	polygon->addChild(polygonColors);

	materialBinding = new SoMaterialBinding();
	materialBinding->ref();
	materialBinding->value = SoMaterialBinding::PER_VERTEX;
    polygon->addChild(materialBinding);

	polygonCoords = new SoCoordinate3();
	polygonCoords->ref();
	polygon->addChild(polygonCoords);

	faceset = new SoIndexedFaceSet();
	faceset->ref();
	polygon->addChild(faceset);


    switchDisplayModeNode = new SoSwitch;
    switchDisplayModeNode->ref();
    switchDisplayModeNode->addChild(polygon);
    switchDisplayModeNode->whichChild=0;


    ////////////////////////////////
    // block stress 
    ////////////////////////////////
    SoSeparator* stressLines = new SoSeparator() ;

    stressLineColor = new SoBaseColor();
    stressLineColor->rgb.setValue(0,0,0);
    stressLineColor->ref();
    stressLines->addChild(stressLineColor);

    stressLineCoords = new SoCoordinate3();
    stressLineCoords->ref();
    stressLines->addChild(stressLineCoords);

    stressLineset = new SoLineSet();
    stressLineset->ref();
    stressLines->addChild(stressLineset);

    switchDisplayModeNode->addChild(stressLines);


	////////////////////////////////
	// block boundary lines
	////////////////////////////////
	SoSeparator* lines = new SoSeparator() ;

	lineColor = new SoBaseColor();
	lineColor->rgb.setValue(0,0,0);
    lineColor->ref();
	lines->addChild(lineColor);

	lineCoords = new SoCoordinate3();
	lineCoords->ref();
	lines->addChild(lineCoords);

	lineset = new SoLineSet();
	lineset->ref();
	lines->addChild(lineset);

	//////////////////////////////////
	// whole block graph
	//////////////////////////////////
	rootGraphBlock = new SoSeparator();
	rootGraphBlock->ref();
    rootGraphBlock->addChild(lines);
 	rootGraphBlock->addChild(switchDisplayModeNode);
}

GraphBlock::~GraphBlock()
{

	// block graph
// 	polygon->removeAllChildren();
	norm->unref();
	normalBinding->unref();
	polygonColors->unref();
	materialBinding->unref();
	polygonCoords->unref();
	faceset->unref();
//     polygon->unref();

    // block boundary lines
    stressLineColor->unref();
    stressLineCoords->unref();
    stressLineset->unref();


	// block boundary lines
    lineColor->unref();
	lineCoords->unref();
	lineset->unref();

    switchDisplayModeNode->unref();
    rootGraphBlock->unref();
}

void GraphBlock::setDisplayMode(DISPLAYMODE mode)
{
    switch(mode)
    {
    case DISPLAY_ACCUMULATION_DISPLACEMENT:
	case DISPLAY_REALTIME_DISPLACEMENT:
	case DISPLAY_REAL_X_DISPLACEMENT:
	case DISPLAY_REAL_Y_DISPLACEMENT:
        {
            switchDisplayModeNode->whichChild=0;
            break;
        }
    case DISPLAY_STRESS:
        {
            switchDisplayModeNode->whichChild=1;
            break;
        }
    }
}

////////////////////////////////////////////////
////
////  bolt element graph
////
////////////////////////////////////////////////
GraphBoltElemnts::GraphBoltElemnts()
{
    rootGraphBolts = new SoSeparator() ;
    rootGraphBolts->ref();

    lineColor = new SoBaseColor();
    lineColor->rgb.setValue(1.0,0,0);
    lineColor->ref();
    rootGraphBolts->addChild(lineColor);


    //     float pts[][3] = {{100.0 , 100.0 , 1} , {100.0 , 0.0 , 1} , {0.0 , 100.0 , 1}};
    boltCoords = new SoCoordinate3() ;
    boltCoords->ref();
    rootGraphBolts->addChild(boltCoords);
    //     meausuredPointsCoords->point.setValues(0 , 3 , pts);

    boltsLineset = new SoLineSet() ;
    boltsLineset->ref();
    rootGraphBolts->addChild(boltsLineset);
    //     measuredPointsLineset->numVertices.setValue(3);

}

GraphBoltElemnts::~GraphBoltElemnts()
{
    rootGraphBolts->unref();
    lineColor->unref();
    boltCoords->unref();
    boltsLineset->unref();
}


////////////////////////////////////////////////
////
////  measured point graph
////
////////////////////////////////////////////////
GraphMeasuredPoints::GraphMeasuredPoints()
{
	rootGraphMPts = new SoSeparator() ;
	rootGraphMPts->ref();

    lineColor = new SoBaseColor();
    lineColor->rgb.setValue(1.0,0,0);
    lineColor->ref();
    rootGraphMPts->addChild(lineColor);


    float pts[][3] = {{100.0 , 100.0 , 1} , {100.0 , 0.0 , 1} , {0.0 , 100.0 , 1}};
	meausuredPointsCoords = new SoCoordinate3() ;
	meausuredPointsCoords->ref();
	rootGraphMPts->addChild(meausuredPointsCoords);
    meausuredPointsCoords->point.setValues(0 , 3 , pts);

	measuredPointsLineset = new SoLineSet() ;
	measuredPointsLineset->ref();
	rootGraphMPts->addChild(measuredPointsLineset);
     measuredPointsLineset->numVertices.setValue(3);

	//mpts = new vector<vector<MeasuredPoint> >();
}

GraphMeasuredPoints::~GraphMeasuredPoints()
{
	//delete mpts;
	rootGraphMPts->unref();
    lineColor->unref();
	meausuredPointsCoords->unref();
	measuredPointsLineset->unref();
}

/////////////////////////////////////////////////
/////
/////  scaler
/////
////////////////////////////////////////////////
SoSeparator* Scaler::_rootGraphBlock = NULL;
SoSwitch* Scaler::_rootSwitch = NULL;
int Scaler::scale = 10;
vector<SoText3*> Scaler::texts;
SoSwitch* Scaler::getScaler( const std::vector<double>& windowInfo , int scale)
{
    Scaler::scale = scale;

#ifdef DEBUGScaler
    printf("prepare to create scaler.\n");
#endif

    if(_rootSwitch==NULL)
    {
        _rootSwitch = new SoSwitch;
        _rootSwitch->ref(); // increment 1, make sure root node will not be destroyed.
#ifdef DEBUGScaler
        printf("create root node.\n");
#endif

    }

#ifdef DEBUGScaler
    printf("root node got.\n");
#endif

    if (_rootGraphBlock)
        _rootGraphBlock->removeAllChildren();
    else
    {
        _rootGraphBlock = new SoSeparator;
        _rootSwitch->addChild(_rootGraphBlock);
    }

#ifdef DEBUGScaler
    printf("scaler root node added to scene graph.\n");
#endif

    double width = windowInfo[1]-windowInfo[0];
    double height = windowInfo[3]-windowInfo[2];
    double centerX = (windowInfo[0]+windowInfo[1])/2;
    double centerY = (windowInfo[2]+windowInfo[3])/2;
    double LBX = centerX - width*0.7;
    double LBY = centerY - height*0.3;

#ifdef DEBUGScaler
    printf("wondow size and position calculated done.\n");
#endif

    SoTransform* trans = new SoTransform;
    trans->translation.setValue(LBX , LBY , 0);
    _rootGraphBlock->addChild(trans);

#ifdef DEBUGScaler
    printf("scaler transformation added to scaler root node.\n");
#endif

    _rootGraphBlock->addChild(_createScalerBar(windowInfo));
    _rootGraphBlock->addChild(_createTexts(windowInfo));

#ifdef DEBUGScaler
    printf("text added done.\n");
#endif

    _rootSwitch->whichChild=0;
#ifdef DEBUGScaler
    printf("root node selected.\n");
#endif

    return _rootSwitch;
}

SoSeparator* Scaler::_createScalerBar( const std::vector<double>& windowInfo )
{
    SoSeparator* root = new SoSeparator;

    SoNormal* norm = new SoNormal;
    SbVec3f norms[] = {SbVec3f(0,0,1.0), SbVec3f(0,0,1.0) , SbVec3f(0,0,1.0) , SbVec3f(0,0,1.0)};
    norm->vector.setValues(0,4,norms);
    root->addChild(norm);

    SoNormalBinding* normalBinding = new SoNormalBinding();
    normalBinding->value = SoNormalBinding::PER_FACE;
    root->addChild(normalBinding);


    float colors[16][3] = {  {0,0,1} , {0,0,1} , {0,1,1} , {0,1,1} , {0,1,1} , {0,1,1} , {0,1,0} , {0,1,0} 
        , {0,1,0} , {0,1,0} , {1,1,0} , {1,1,0} , {1,1,0} , {1,1,0} , {1,0,0} , {1,0,0} };

    SoMaterial* polygonColors = new SoMaterial() ;
    polygonColors->diffuseColor.setValues(0 , 16 , colors);
    root->addChild(polygonColors);

    SoMaterialBinding* materialBinding = new SoMaterialBinding();
    materialBinding->value = SoMaterialBinding::PER_VERTEX;
    root->addChild(materialBinding);

    double width = windowInfo[1]-windowInfo[0];
    double height = windowInfo[3]-windowInfo[2];
    double centerX = (windowInfo[0]+windowInfo[1])/2;
    double centerY = (windowInfo[2]+windowInfo[3])/2;

    double detY = 0.6*height;
    double detX = detY*0.1; 

    float pts[16][3] = { {0,0,0 }  ,      {detX,0,0 } ,       {detX,detY*0.25,0}, {0,detY*0.25,0}\
                        ,{0,detY*0.25,0 },{detX,detY*0.25,0 },{detX,detY*0.5 ,0}, {0,detY*0.5,0 }\
                        ,{0,detY*0.5 ,0 },{detX,detY*0.5 ,0 },{detX,detY*0.75,0}, {0,detY*0.75,0}
                        ,{0,detY*0.75,0 },{detX,detY*0.75,0 },{detX,detY,0 },     {0,detY,0 }};

    SoCoordinate3* polygonCoords = new SoCoordinate3();
    polygonCoords->point.setValues(0 , 16 , pts);
    root->addChild(polygonCoords);

    SoFaceSet* face = new SoFaceSet();
    int nums[] = {4 ,4 , 4, 4};
    face->numVertices.setValues(0 , 4 , nums);
    root->addChild(face);
    return root;
}

SoSeparator* Scaler::_createTexts( const std::vector<double>& windowInfo )
{
    SoSeparator* textRoot = new SoSeparator;
    double width = windowInfo[1]-windowInfo[0];
    double height = windowInfo[3]-windowInfo[2];
    double detY = height;
    double detX = 0.12*height;

    double disInterval = height/float(scale);

    SoFont* font = new SoFont;
    font->name.setValue("Times-Roman");
    font->size.setValue(height/300.0*9);

    texts.resize(10);

    for( int i=0 ; i<10 ; i++)
    {
        SoSeparator* g = new SoSeparator;

        SoTransform* t = new SoTransform;
        t->translation.setValue(detX , detY*0.06*i , 0);
        g->addChild(t);

        SoBaseColor* c = new SoBaseColor;
        c->rgb.setValue(0,0,0);
        g->addChild(c);

        g->addChild(font);

        SoText3* text = new SoText3;
        char str[100];
        sprintf(str , "%lf" , disInterval*0.1*i);
        text->string.setValue(str);
        texts[i] = text;
        g->addChild(text);

        textRoot->addChild(g);
    }
    return textRoot;
}

void Scaler::setScale( double value )
{
    if(_rootSwitch==NULL)
        return;
    char str[200];

	double base = sqrt(value)/10.0;
	
    for( int i=0; i<10 ; i++)
    {
		if(getIfDisplayWithQuadraticCurve())
			sprintf(str , "%lf" , (base*i)*(base*i));
		else
			sprintf(str , "%lf" , base*i);
        texts[i]->string.setValue(str);
    }
	msg2File("Scale::setScale done.\n");
}

/////////////////////////////////////////////////
/////
/////  scene graph
/////
////////////////////////////////////////////////

SceneGraph::SceneGraph()
{
//	blocks = new vector<GraphBlock>();
    blocks = NULL;
    graphMeasuredPoints = NULL;

	rootScene = new SoSeparator();
	rootScene->ref();
}

SceneGraph::~SceneGraph()
{
    rootScene->unref();
	delete []blocks ;
	delete graphMeasuredPoints ;
}

void SceneGraph::setDisplayMode(DISPLAYMODE mode)
{
    for(int i=0 ; i<blocksNum ; i++)
        blocks[i].setDisplayMode(mode);

	if (mode ==DISPLAY_STRESS)
		Scaler::setVisible(false);
	else
		Scaler::setVisible(true);
//     switch(mode)
//     {
//     case DISPLAY_ACCUMULATION_DISPLACEMENT:
//         {
//             Scaler::setVisible(true);
//             break;
//         }
//     case DISPLAY_STRESS:
//         {
//             Scaler::setVisible(false);
//             break;
//         }
//     }
}


void SceneGraph::resetRootScene(const vector<int> schema , const vector<double> windowInfo )
{
#ifdef DEBUGSceneGraph
    printf("start reset scene graph.\n");
#endif
    rootScene->removeAllChildren();

    /////////////////////////////////
    //  blocks
    /////////////////////////////////
    blocksNum = schema[0];
    if(blocks){
        delete []blocks;
        blocks = NULL;
    }
#ifdef DEBUGSceneGraph
    printf("blocks init done.\n");
#endif

    blocks = new GraphBlock[blocksNum];
    for(int i=0 ; i<schema[0] ; i++) 
    {
        assert(blocks[i].rootGraphBlock);
        rootScene->addChild(blocks[i].rootGraphBlock);
    }

#ifdef DEBUGSceneGraph
    printf("blocks add done.\n");
#endif
        
    bolts = new GraphBoltElemnts();
    rootScene->addChild(bolts->rootGraphBolts);

#ifdef DEBUGSceneGraph
    printf("bolts add done.\n");
#endif
    /////////////////////////////////
    //  measured points
    /////////////////////////////////
    if(graphMeasuredPoints){
        delete graphMeasuredPoints; 
        graphMeasuredPoints=NULL;
    }
    graphMeasuredPoints = new GraphMeasuredPoints;
    assert(graphMeasuredPoints);
 	//graphMeasuredPoints->mpts->resize(schema[3]);
  	rootScene->addChild(graphMeasuredPoints->rootGraphMPts);

#ifdef DEBUGSceneGraph
    printf("measured points add done.\n");
#endif
    /////////////////////////////////
    //  scaler
    /////////////////////////////////
    rootScene->addChild(Scaler::getScaler(windowInfo , 10));
#ifdef DEBUGSceneGraph
    printf("all add done.\n");
#endif
}

/////////////////////////////////////////////////
/////
/////  block updater
/////
////////////////////////////////////////////////



/////////////////////////////////////////////////
/////  tool class "ScaleCalculator" for block updater
////////////////////////////////////////////////
double ScaleCalculator4AccumulatedDisplacement::calculateScale(double x1, double y1, double x2, double y2)
{
	const vector<double>& windowInfo = BlockUpdatater::getWindowInfo();
	double tmp = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
	double scale = getScale4Display();
	double t1 = scale==0? tmp/getAccumulatedDisplacement() : tmp/scale;
	//double t1 = tmp/((windowInfo[3]-windowInfo[2])/90.0);
	if(t1>1) t1 = 1;
	if(getIfDisplayWithQuadraticCurve())
		t1 = t1*t1;   // use quadratic curve
	return t1;
}
double ScaleCalculator4RealTimeDisplacement::calculateScale(double x1, double y1, double x2, double y2)	
{
	double tmp = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
	double t1 = tmp/getRealTimeDisplacement();
	if(t1>1) t1 = 1;
	if(getIfDisplayWithQuadraticCurve())
		t1 = t1*t1;   // use quadratic curve
	return t1;
}

double ScaleCalculator4RealXDisplacement::calculateScale(double x1, double y1, double x2, double y2)
{
	double tmp = fabs(x1-x2);//sqrt((x1-x2)*(x1-x2));
	double t1 = tmp/getRealTimeDisplacement();
	if(t1>1) t1 = 1;
	if(getIfDisplayWithQuadraticCurve())
		t1 = t1*t1;   // use quadratic curve
	return t1;
}
double ScaleCalculator4RealYDisplacement::calculateScale(double x1, double y1, double x2, double y2)
{
	double tmp = fabs(y1-y2);//sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
	double t1 = tmp/getRealTimeDisplacement();
	if(t1>1) t1 = 1;
	if(getIfDisplayWithQuadraticCurve())
		t1 = t1*t1;   // use quadratic curve
	return t1;
}

ScaleCalculator4AccumulatedDisplacement accumulatedDisplacementCalc;
ScaleCalculator4RealTimeDisplacement realTimeDisplacementCalc;
ScaleCalculator4RealXDisplacement realXDisplacementCalc;
ScaleCalculator4RealYDisplacement realYDisplacementCalc;

ScaleCalculator* BlockUpdatater::scaleCalculator = &accumulatedDisplacementCalc;
vector<double> BlockUpdatater::windowInfo = vector<double>();
double BlockUpdatater::scale = 0;  // scale for color
FrameData* BlockUpdatater::originalGraph = NULL;
vector<SbVec3f> BlockUpdatater::coords = vector<SbVec3f>();
int BlockUpdatater::tmpCount=0;

void BlockUpdatater::setScale( double scaleValue)
{
    scale = scaleValue;
}

void BlockUpdatater::reset(  FrameData* originalGraph)
{
	BlockUpdatater::scale = 1;
	BlockUpdatater::originalGraph = originalGraph;
    _calcWindowInfoFromFrame();
}

void BlockUpdatater::_calcWindowInfoFromFrame()
{
    assert(originalGraph!=NULL && originalGraph->blocks.size()>0) ;
    const vector<Block>& blocks = originalGraph->blocks;
    windowInfo.resize(4);
    windowInfo[0] = windowInfo[1] = originalGraph->blocks[0].vertices[0].first;
    windowInfo[2] = windowInfo[3] = originalGraph->blocks[0].vertices[0].second;
    for(int i=0 ; i<blocks.size() ; i++)
    {
        const vector<pair<double,double> >& pts = blocks[i].vertices;
        for(vector<pair<double,double> >::const_iterator it = pts.begin() ; it!=pts.end() ; it++){
            if(it->first < windowInfo[0]) windowInfo[0] = it->first;
            if(it->first > windowInfo[1]) windowInfo[1] = it->first;
            if(it->second< windowInfo[2]) windowInfo[2] = it->second;
            if(it->second> windowInfo[3]) windowInfo[3] = it->second;
        }
    }
}

void BlockUpdatater::update(FrameData* frame , GraphBlock* graphBlock , int blockIdx , DISPLAYMODE mode)
{
    switch(mode)
    {
		case DISPLAY_ACCUMULATION_DISPLACEMENT:
		{
			setScaleCalculator(&accumulatedDisplacementCalc);
			_updatePolygonColor(frame , graphBlock , blockIdx);
			_updateLines(frame , graphBlock , blockIdx);
			break;
		}
		case DISPLAY_REALTIME_DISPLACEMENT:
		{
			setScaleCalculator(&realTimeDisplacementCalc);
			_updatePolygonColor(frame , graphBlock , blockIdx);
			_updateLines(frame , graphBlock , blockIdx);
			break;
		}
		case DISPLAY_REAL_X_DISPLACEMENT :
		{
			setScaleCalculator(&realXDisplacementCalc);
			_updatePolygonColor(frame , graphBlock , blockIdx);
			_updateLines(frame , graphBlock , blockIdx);
			break;
		}
		case DISPLAY_REAL_Y_DISPLACEMENT :
        {
			setScaleCalculator(&realYDisplacementCalc);
 	        _updatePolygonColor(frame , graphBlock , blockIdx);
	        _updateLines(frame , graphBlock , blockIdx);
            break;
        }
    case(DISPLAY_STRESS):
        {
            _updateStress(frame , graphBlock , blockIdx);
            _updateLines(frame , graphBlock , blockIdx);
            break;
        }
    }
}

void BlockUpdatater::_calcStressCrossCoords( Block& block , vector<pair<double , double> >& coords , double crossHeight )
{
    /*--------------------------------------*/
    /* this function is almost copied from dg05 by Dr.Shi
     * I just changed some variables' names
     */
    double p[3];
    double q[3][3];
    double b1 , b2 , a1 , c1 , d0 , d1;

    if(block.stressXY<=.00000001)
    {
        p[1]    = block.stressX;
        p[2]    = block.stressY;
        q[1][1] = 1;
        q[1][2] = 0;
        q[2][1] = 0;
        q[2][2] = 1;
    }
    else
    {
        b1 =   (-1)*(block.stressX + block.stressY);
        c1 = block.stressX*block.stressY - block.stressXY*block.stressXY;
        d0 = b1*b1 - 4*c1;
        d1   = sqrt(d0 + 0.0000000001);
        p[1] = (-b1 - d1)/2;
        p[2] = (-b1 + d1)/2;

        for (int j=1; j<= 2; j++)
        {
            b2 = block.stressX - p[j];
            if (fabs(b2) <= .00000001) b2 = .00000001;
            q[j][1] =  -block.stressXY/b2;
            q[j][2] = 1;
            a1      = sqrt(q[j][1]*q[j][1] + q[j][2]*q[j][2]);
            q[j][1] = q[j][1]/a1;
            q[j][2] = q[j][2]/a1;
        }
    }

    double centerX=0 , centerY=0;
    const vector<pair<double , double> >& vertices = block.vertices;
    assert(vertices.size()>=3);
    if(vertices.size()<3)   return;

    for(vector<pair<double,double> >::const_iterator it = vertices.begin() ; 
        it!=vertices.end() ; it++)
    {
        centerX += it->first;
        centerY += it->second;
    }
    centerX /= vertices.size();
    centerY /= vertices.size();

    coords.resize(4);
    double x1 , y01 ;
    for (int j=1; j<= 2; j++)
    {
        x1 =q[j][1]*p[j]/maxStress*crossHeight;
        y01=q[j][2]*p[j]/maxStress*crossHeight;
        coords[2*(j-1)].first=centerX+x1;
        coords[2*(j-1)].second=centerY+y01;

        if(p[j]<=0)
        {
            coords[2*(j-1)+1].first=centerX-x1;
            coords[2*(j-1)+1].second=centerY-y01;
        }
        else
        {
            coords[2*(j-1)+1].first=centerX;
            coords[2*(j-1)+1].second=centerY;
        }
    }
}

void BlockUpdatater::_updateStress(FrameData* frame , GraphBlock* graphBlock , int blockIdx )
{
    scanAndUpdateMaxStress(frame); 
    double len = (windowInfo[3]-windowInfo[2])/20;
    vector<pair<double , double> > coords;
    BlockUpdatater::_calcStressCrossCoords(frame->blocks[blockIdx] , coords , len);

    graphBlock->stressLineCoords->point.setNum(4);
    SbVec3f* p = graphBlock->stressLineCoords->point.startEditing();
    for( int i=0 ; i<4 ; i++)
        p[i].setValue(coords[i].first , coords[i].second , 0);
    int nums[] = {2 ,2};
    graphBlock->stressLineset->numVertices.setValues(0 , 2 , nums);
}

void BlockUpdatater::_calcPolygonColor(FrameData* frame , GraphBlock* graphBlock , int blockIdx , SbColor* colors)
{
	double x1, y1, x2, y2;
	const vector<pair<double,double> >& pts1 = frame->blocks[blockIdx].vertices;
	const vector<pair<double,double> >& originPts = originalGraph->blocks[blockIdx].vertices;

	assert(pts1.size()==originPts.size());
	for(int i=0 ; i<pts1.size();i++)
	{
		const pair<double , double>& p1= pts1[i];
		const pair<double , double>& p2= originPts[i];
		double t1 = scaleCalculator->calculateScale(p1.first, p1.second, p2.first, p2.second);
		if(t1>0.75)                 { t1-=0.75 ; colors[i].setValue( 1    , 1-4*t1 , 0      );}
		else if(t1<=0.75 && t1>0.5) { t1-=0.5  ; colors[i].setValue( 4*t1 , 1      , 0      );}
		else if(t1<=0.5 && t1>0.25) { t1-=0.25 ; colors[i].setValue( 0    , 1      , 1-4*t1 );}
		else						{			 colors[i].setValue( 0    , 4*t1   , 1      );}
	}
#ifdef DEBUGFRAMEMAKER
	char tmpStr[200];
	sprintf(tmpStr , "%d colors got.\n" , colors);
#endif
}




int BlockUpdatater::_getVectorUp(const SbVec3f& p1 , const SbVec3f& p2 , const SbVec3f& p3)
{
// 	double ax = p1[0]-p2[0];
// 	double ay = p1[1]-p2[1];
// 	double bx = p2[0]-p3[0];
// 	double by = p2[1]-p3[1];
//	double tmp = ax*by-ay*bx;  // cross vector v1*v2
//  return tmp>0?1:-1
	return (p1[0]-p2[0])*(p2[1]-p3[1])-(p1[1]-p2[1])*(p2[0]-p3[0])>0?1:-1;
}


bool BlockUpdatater::_ifConcave(const vector<SbVec3f>& pts)
{
	assert(pts.size()>2);
	int size = pts.size();
	if(pts[0]==pts[size-1])
		size-=1;
	
	double res = _getVectorUp(pts[0] , pts[1] , pts[2]);
	for( int i=0 ;i<size; i++)
	{
		if( _getVectorUp(pts[i],pts[(i+1)%size] , pts[(i+2)%size])!=res)
			return false;
	}
	return true;
}

void BlockUpdatater::_triangulationCallback(void * v0, void * v1, void * v2, void * cbdata)
{
	SbVec3f * vtx0 = (SbVec3f *)v0;
	SbVec3f * vtx1 = (SbVec3f *)v1;
	SbVec3f * vtx2 = (SbVec3f *)v2;

	coords.push_back(*vtx0);
	coords.push_back(*vtx1);
	coords.push_back(*vtx2);
#ifdef DEBUGFRAMEMAKER
    msg("one triangle is tesselated out.\n");
#endif
}

void BlockUpdatater::_triangulate(vector<SbVec3f>& pts)
{
	assert(pts.size()>3);
	coords.clear();

    int size = pts.size();
    SbVec3f p1 = pts[0];
    SbVec3f p2 = pts.back();

//     char str[200];
//     sprintf(str , "(%f , %f)<--->(%f , %f)\n" , p1[0] , p1[1] , p2[0] , p2[1]);
//     msg(str);
// 
//     if(p1[0]==p2[0] && p1[1]==p2[1]) 
//     {size-=1; msg("first and last point are same");}
// 

    Vector2dVector vec2d;
    vec2d.resize(size);
    for( int i=0; i<size ; i++)
        vec2d[i].Set(pts[i][0],pts[i][1]);

    Vector2dVector res;
    Triangulate::Process( vec2d , res);
    coords.resize(res.size());
    for( int i=0; i<res.size() ; i++)
    {
        coords[i][0] = res[i].GetX();
        coords[i][1] = res[i].GetY();
        coords[i][2] = -1;
    }

// 	SbTesselator tesselator(_triangulationCallback , NULL);
// 	tesselator.beginPolygon();
// 	for(int i=0 ; i<size ; i++)
// 		tesselator.addVertex(pts[i] , &pts[i]);
// 	tesselator.endPolygon();
	assert(int(coords.size())%3==0);
// #ifdef DEBUGFRAMEMAKER
// 	char tmpStr[200];
// 	sprintf(tmpStr , "tesselate done. %d triangles are tesselated out.\n", int(coords.size())/3);
// 	msg(tmpStr);
// #endif
}

int find_pos(const vector<SbVec3f>& pts , SbVec3f p)
{
	// if found , return n (n>0) , else return -1
	for(int i=0 ; i<pts.size() ; i++)
		if( pts[i]==p )
			return i;
	return -1;
}

void BlockUpdatater::_calcColors4ConcavePolygon(const vector<SbVec3f>& pts , SbColor* colors , SbColor* resultColors)
{
	// calculate colors for points after tesselation

	double* percents = new double[coords.size()+1];
	double sum = 0;

	for(int i=0 ; i<coords.size() ; i++)
	{
		int pos = find_pos(pts , coords[i]);
		if( pos>0 ){ resultColors[i]=colors[pos]; continue;}
		else // the point is not in pts
		{
			// calculate distance to every point in original concave polygon
			for(int j=0 ; j<pts.size() ; j++)
			{
				percents[j] = (coords[i][0]-pts[j][0])*(coords[i][0]-pts[j][0])
					+(coords[i][1]-pts[j][1])*(coords[i][1]-pts[j][1]);
				sum+=percents[j];
			}
			for(int j=0 ; j<pts.size() ; j++)
				if( percents[j]==0)	percents[j]=sum;
				else percents[j] = sum/percents[j];

			// get percent for every point in original convex polygon to the current coords[i]
			sum=0;
			for(int j=0 ; j<pts.size() ; j++)
				sum+=percents[j];
			for(int j=0 ; j<pts.size() ; j++)
				percents[j]/=sum;

			// get color for this point
			SbColor& color = resultColors[i];
			color[0]=color[1]=color[2]=0;
			for(int j=0 ; j<pts.size() ; j++)
				for( int k=0 ; k<3 ; k++)
					color[k]+=percents[j]*colors[j][k];
		}
	}
	delete []percents;
}

void BlockUpdatater::_updatePolygonColor(FrameData* frame , GraphBlock* graphBlock , int blockIdx
										 ,DISPLAYMODE mode)
{
	const vector<pair<double,double> >& tmpPts = frame->blocks[blockIdx].vertices;

    if(tmpPts.size()<=2)
    {
        int a=0;
        int b=3;
    }

    assert(tmpPts.size()>2);

	SbColor* colors = new SbColor[tmpPts.size()+1];
	_calcPolygonColor(frame , graphBlock , blockIdx , colors);
	
	SbVec3f* pts = new SbVec3f[tmpPts.size()+1];
	for(int i=0;i<tmpPts.size();i++)
	{
		pts[i][0] = tmpPts[i].first;
		pts[i][1] = tmpPts[i].second;
		pts[i][2] = -1;
	}
	vector<SbVec3f> vecPts(pts , pts+tmpPts.size());
	assert(vecPts.size()==tmpPts.size());


//     char tmpStr[200];
	if(_ifConcave(vecPts))
	{
        //sprintf(tmpStr , "\t%d-th polygon is convex.\n" , blockIdx);
        //msg(tmpStr);

        graphBlock->norm->vector.setNum(tmpPts.size());
        SbVec3f * p = graphBlock->norm->vector.startEditing();
        for(int i=0 ; i<tmpPts.size(); i++) p[i].setValue(0,0,1.0);
        graphBlock->norm->vector.finishEditing();

        graphBlock->polygonCoords->point.setValues(0 , tmpPts.size() , pts);
		
        graphBlock->faceset->coordIndex.setNum(tmpPts.size()+1);
        int* pIdx = graphBlock->faceset->coordIndex.startEditing();
        for(int i=0; i<tmpPts.size(); i++) pIdx[i]=i;
        pIdx[tmpPts.size()] = SO_END_FACE_INDEX;
        graphBlock->faceset->coordIndex.finishEditing();

		graphBlock->polygonColors->diffuseColor.setValues(0 , tmpPts.size() , colors);
	}
	else // the polygon is concave
	{
		_triangulate(vecPts);
        graphBlock->polygonCoords->point.setNum(coords.size());
        SbVec3f* pCoord = graphBlock->polygonCoords->point.startEditing();
        for (int i=0 ; i<coords.size() ; i++)
            pCoord[i].setValue(coords[i][0] , coords[i][1] , coords[i][2]);
        graphBlock->polygonCoords->point.finishEditing();

        assert(int(coords.size())%3==0);
        const int size = int(coords.size())/3;
        graphBlock->faceset->coordIndex.setNum(size*4);
        int* pIdx = graphBlock->faceset->coordIndex.startEditing();
        for(int i=0 , j=0; i<coords.size(); i++)
        {
            pIdx[j++]=i;
            if((i+1)%3==0)
                pIdx[j++] = SO_END_FACE_INDEX;
        }
        graphBlock->faceset->coordIndex.finishEditing();

        graphBlock->norm->vector.setNum(coords.size());
        SbVec3f * p = graphBlock->norm->vector.startEditing();
        for(int i=0 ; i<coords.size(); i++) p[i].setValue(0,0,1.0);
        graphBlock->norm->vector.finishEditing();
		
		SbColor *TColors = new SbColor[coords.size()+1];
		_calcColors4ConcavePolygon(vecPts , colors , TColors);
		graphBlock->polygonColors->diffuseColor.setValues(0 , coords.size() , TColors);
		delete []TColors;
	}
// 
//     sprintf(tmpStr , "%d-th polygon was parsed out.\n" , blockIdx);
//     msg(tmpStr);
// 
	delete []pts;
	delete []colors;
}
void BlockUpdatater::_updateLines(FrameData* frame , GraphBlock* graphBlock , int blockIdx)
{
	const vector<pair<double , double> >& pts = frame->blocks[blockIdx].vertices;
	const int size = pts.size();

	SbVec3f* TmpPts = new SbVec3f[size+1];
	for( int i=0 ; i<size ; i++)
	{
		TmpPts[i][0] = pts[i].first;
		TmpPts[i][1] = pts[i].second;
		TmpPts[i][2] = 0;
	}
    TmpPts[size] = TmpPts[0];

	if( pts.front()==pts.back())
	{
		graphBlock->lineCoords->point.setValues(0 , size , TmpPts);
        int nums[] = {size};
		graphBlock->lineset->numVertices.setValues(0 , 1 , nums);
	}
	else
	{
		graphBlock->lineCoords->point.setValues(0 , size+1 , TmpPts);
        int nums[] = {size+1};
		graphBlock->lineset->numVertices.setValues(0 , 1 , nums);
	}
    delete []TmpPts;
}


//////////////////////////////////////////////////////
/////
///// bolt elements Updater
/////
//////////////////////////////////////////////////////
void BoltElementsUpdater::update(FrameData* frame , SceneGraph* sceneGraph)
{
    const int num = frame->bolts.size();
    GraphBoltElemnts* graphBolts = sceneGraph->bolts;
    if (num==0 ) return;

    SoCoordinate3* coords = graphBolts->boltCoords;
    coords->point.setNum(num*2);
    SbVec3f* p = coords->point.startEditing();

    vector<BoltElement>& bolts = frame->bolts;
    for( int i=0 ; i<num ; i++)
    {
        p[2*i].setValue(bolts[i].startPoint.first , bolts[i].startPoint.second , 0.5);
        p[2*i+1].setValue(bolts[i].endPoint.first , bolts[i].endPoint.second , 0.5);
    }
    coords->point.finishEditing();

    graphBolts->boltsLineset->numVertices.setNum(num);
    int* pNum = graphBolts->boltsLineset->numVertices.startEditing();
    for(int i=0 ; i<num ; i++)  pNum[i] = 2;
    graphBolts->boltsLineset->numVertices.finishEditing();
}


//////////////////////////////////////////////////////
/////
///// Points Updater
/////
//////////////////////////////////////////////////////
void PointsUpdater::update(FrameData* frame , SceneGraph* sceneGraph)
{
    const int num = frame->measuredPoints.size();
    GraphMeasuredPoints* graphMPts = sceneGraph->graphMeasuredPoints;
//    if (graphMPts->mpts->size()==0 ) return;
//    const int steps = (*(graphMPts->mpts))[0].size();
	const int steps = frame->frameNo;

    SoCoordinate3* coords = graphMPts->meausuredPointsCoords;
	coords->point.setNum(num*(steps));
// 	coords->point.setNum(num*(frame->frameNo));
// 	coords->point.setNum(num*2);
    SbVec3f* p = coords->point.startEditing();
//	for( int i=0 ; i<steps ; i++)
	for( int i=0 ; i<num ; i++)
    {
// 		p[i*2].setValue(100.0, frame->frameNo+0.0, 1.0);
// 		p[i*2+1].setValue(101.0, frame->frameNo+0.0, 1.0);
// //         vector<MeasuredPoint>& tmpPts = (*(graphMPts->mpts))[i];
// //         tmpPts.push_back( frame->measuredPoints[i] );
// 
		std::vector<SbVec3f>& tmpPts = getMeasuredPointCoords4Idx(i);
        for(int j=0 ; j<steps ; j++)
            p[i*steps+j].setValue(tmpPts[j][0], tmpPts[j][1], 1.0);
    }
    coords->point.finishEditing();
//	SbVec3f* pts = 
    
    graphMPts->measuredPointsLineset->numVertices.setNum(num);
    int* pNum = graphMPts->measuredPointsLineset->numVertices.startEditing();
	for(int i=0 ; i<num ; i++)  pNum[i] = steps;
// 	for(int i=0 ; i<num ; i++)  pNum[i] = 2;
    graphMPts->measuredPointsLineset->numVertices.finishEditing();

// 	float pts[][3] = {{100.0+frame->frameNo , 100.0 , 1} , {100.0 , 0.0 , 1} , {0.0 , 100.0 , 1}};
// 	graphMPts->meausuredPointsCoords->point.setValues(0,3,pts);
// 	graphMPts->measuredPointsLineset->numVertices.setValue(3);
}



/////////////////////////////////////////////////
/////
/////  frame maker
/////
////////////////////////////////////////////////



FrameMaker::FrameMaker(void)
{
	_rootSceneGraph = new SceneGraph();
}

FrameMaker::~FrameMaker(void)
{
    removeGraphFromScene();
	delete _rootSceneGraph;
}

bool FrameMaker::reset(const FrameData& frame,  const vector<int>& schema)
{
	_firstFrame = frame;
#ifdef DEBUGFRAMEMAKER
    msg("first frame get done.\n");
#endif
    BlockUpdatater::reset( &_firstFrame);
#ifdef DEBUGFRAMEMAKER
    msg("frameMaker reset done.\n");
#endif
    _rootSceneGraph->resetRootScene(schema , BlockUpdatater::getWindowInfo());
    const vector<double>& windowInfo = BlockUpdatater::getWindowInfo();
    setScale( (windowInfo[3]-windowInfo[2])/90 );
#ifdef DEBUGFRAMEMAKER
    msg("root secene reset done.\n");
#endif
	updateGraph(_firstFrame);
#ifdef DEBUGFRAMEMAKER
    msg("first frame draw done.\n");
#endif

	return true;
}

bool FrameMaker::setRootSceneNode(SoSeparator* node)
{
	_freeCADrootNode = node;
	_freeCADrootNode->addChild(_rootSceneGraph->rootScene);
#ifdef DEBUGFRAMEMAKER
	msg("frameMaker set root freecad node done.\n");
#endif
	return true;
}

void FrameMaker::removeGraphFromScene()
{
    _freeCADrootNode->removeChild(_rootSceneGraph->rootScene);
}

void FrameMaker::setScale(double value)
{
// 	clearAccumulatedDisplacement();
    Scaler::setScale(value);
    BlockUpdatater::setScale(value);
}

void FrameMaker::updateScale(DISPLAYMODE mode)
{
	switch(mode)
	{
	case DISPLAY_REALTIME_DISPLACEMENT:
	case DISPLAY_REAL_X_DISPLACEMENT :
	case DISPLAY_REAL_Y_DISPLACEMENT :
		{
			Scaler::setScale(getRealTimeDisplacement());
			break;
		}
	case(DISPLAY_ACCUMULATION_DISPLACEMENT):
		{
			double scale = getScale4Display();
			double tmp = scale==0 ? getAccumulatedDisplacement(): scale;
			Scaler::setScale(tmp);
			break;
		}
	case(DISPLAY_STRESS):
		{
			break;
		}
	}
}

bool FrameMaker::updateGraph(FrameData frame , DISPLAYMODE mode)
{
    _rootSceneGraph->setDisplayMode(mode);
	assert(frame.blocks.size()>0);
	setFrameIdx(frame.frameNo);
	updateScale(mode);
	int size = frame.blocks.size();
	for( int i=0 ; i<size ; i++)
        BlockUpdatater::update(&frame , &(_rootSceneGraph->blocks[i]) , i , mode);

    BoltElementsUpdater::update(&frame , _rootSceneGraph);
    PointsUpdater::update(&frame , _rootSceneGraph);

#ifdef DEBUGFRAMEMAKER
	msg("frameMaker parse one graph done.\n");
#endif
    return true;
}
}   // namespace DDAGui